/*
 *  v4l2cam.c
 *  libCV
 *
 *  Created by Gregory Koch on 14/03/11.
 *  Copyright 2011 IFlyBotIV. All rights reserved.
 *
 */

#include "v4l2cam.h"

static int xioctl(int fh, unsigned long int request, void *arg)
{
	int r;
    
	do {
		r = ioctl(fh, request, arg);
	} while (-1 == r && EINTR == errno);
    
	return r;
}

static void errno_exit(const char *s)
{
	fprintf(stderr, "%s error %d, %s\n", s, errno, strerror(errno));
	exit(EXIT_FAILURE);
}


static int read_frame(v4l2cam_t cam)
{
	struct v4l2_buffer buf;
    
    CLEAR(buf);
    
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;
    
    if (-1 == xioctl(cam.fdv, VIDIOC_DQBUF, &buf)) {
        switch (errno) {
			case EAGAIN:
				return 0;
                
			case EIO:
				/* Could ignore EIO, see spec. */
                
				/* fall through */
                
			default:
				errno_exit("VIDIOC_DQBUF");
        }
    }
    
    assert(buf.index < cam.n_buffers);
    
    switch(cam.framebuf.type)
    {
        case IMG_RGB24:
            ccvt_yuyv_rgb24(cam.framebuf.size.width, cam.framebuf.size.height,
                            cam.buffers[buf.index].start, cam.framebuf.data);
            break;
            
        case IMG_GREYSCALE8:
            ccvt_yuyv_grey8(cam.framebuf.size.width, cam.framebuf.size.height,
                            cam.buffers[buf.index].start, cam.framebuf.data);
            break;
    }
    
    if (-1 == xioctl(cam.fdv, VIDIOC_QBUF, &buf))
        errno_exit("VIDIOC_QBUF");
    
	return 1;
}


v4l2cam_t v4l2cam_open(char* device, unsigned int format)
{
    v4l2cam_t cam;
    struct stat st;
    struct v4l2_capability cap;
	struct v4l2_cropcap cropcap;
	struct v4l2_crop crop;
	struct v4l2_format fmt;
	unsigned int min, i;
    struct v4l2_requestbuffers req;
    struct v4l2_buffer buf;
    enum v4l2_buf_type type;
    struct v4l2_streamparm setfps;
    
    switch(format)
    {
        case V4L2_RGB24_640_480:
            cam.framebuf = init_image(IMG_RGB24, init_size(640, 480));
            break;
        case V4L2_GREY8_640_480:
            cam.framebuf = init_image(IMG_GREYSCALE8, init_size(640, 480));
            break;
        case V4L2_RGB24_320_240:
            cam.framebuf = init_image(IMG_RGB24, init_size(320, 240));
            break;
        case V4L2_GREY8_320_240:
            cam.framebuf = init_image(IMG_GREYSCALE8, init_size(320, 240));
            break;
        case V4L2_RGB24_160_120:
            cam.framebuf = init_image(IMG_RGB24, init_size(160, 120));
            break;
        case V4L2_GREY8_160_120:
            cam.framebuf = init_image(IMG_GREYSCALE8, init_size(160, 120));
            break;
        default:
            fprintf(stderr, "libCV : v4lcam : unknow format\n");
            exit(EXIT_FAILURE);
            break;
    }
    
    cam.format = V4L2_PIX_FMT_YUYV;
    
    /* Open camera */
	if (-1 == stat(device, &st)) {
		fprintf(stderr, "Cannot identify '%s': %d, %s\n",
                device, errno, strerror(errno));
		exit(EXIT_FAILURE);
	}
    
	if (!S_ISCHR(st.st_mode)) {
		fprintf(stderr, "%s is no device\n", device);
		exit(EXIT_FAILURE);
	}
    
	cam.fdv = open(device, O_RDWR /* required */ | O_NONBLOCK, 0);
    
	if (-1 == cam.fdv) {
		fprintf(stderr, "Cannot open '%s': %d, %s\n",
                device, errno, strerror(errno));
		exit(EXIT_FAILURE);
	}
    
    
    /* Init device */
	if (-1 == xioctl(cam.fdv, VIDIOC_QUERYCAP, &cap)) {
		if (EINVAL == errno) {
			fprintf(stderr, "%s is no V4L2 device\n",
                    device);
			exit(EXIT_FAILURE);
		} else {
			errno_exit("VIDIOC_QUERYCAP");
		}
	}
    
	if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
		fprintf(stderr, "%s is no video capture device\n",
                device);
		exit(EXIT_FAILURE);
	}
    
    
    if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
        fprintf(stderr, "%s does not support streaming i/o\n",
                device);
        exit(EXIT_FAILURE);
    }
    
    
    
	/* Select video input, video standard and tune here. */
    
    
	CLEAR(cropcap);
    
	cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    
	if (0 == xioctl(cam.fdv, VIDIOC_CROPCAP, &cropcap)) {
		crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		crop.c = cropcap.defrect; /* reset to default */
        
		if (-1 == xioctl(cam.fdv, VIDIOC_S_CROP, &crop)) {
			switch (errno) {
                case EINVAL:
                    /* Cropping not supported. */
                    break;
                default:
                    /* Errors ignored. */
                    break;
			}
		}
	} else {
		/* Errors ignored. */
	}
    
    
	CLEAR(fmt);
    
	fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width       = cam.framebuf.size.width;
    fmt.fmt.pix.height      = cam.framebuf.size.height;
    fmt.fmt.pix.pixelformat = cam.format;
    fmt.fmt.pix.field       = V4L2_FIELD_INTERLACED;
    
    if (-1 == xioctl(cam.fdv, VIDIOC_S_FMT, &fmt))
        errno_exit("VIDIOC_S_FMT");
    
    /* Note VIDIOC_S_FMT may change width and height. */
    
    
	/* Buggy driver paranoia. */
	min = fmt.fmt.pix.width * 2;
	if (fmt.fmt.pix.bytesperline < min)
		fmt.fmt.pix.bytesperline = min;
	min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
	if (fmt.fmt.pix.sizeimage < min)
		fmt.fmt.pix.sizeimage = min;
    
    
    /* Set framerate */
    memset(&setfps, 0, sizeof(struct v4l2_streamparm));
    setfps.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    setfps.parm.capture.timeperframe.numerator=1;
    setfps.parm.capture.timeperframe.denominator=30;
    
    if (xioctl(cam.fdv, VIDIOC_S_PARM, &setfps) == -1)
    {
        fprintf(stderr, "Can't set framerate");
        exit(EXIT_FAILURE);
    }
    
    
    /* init mmap */
	CLEAR(req);
    
	req.count = 2;
	req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	req.memory = V4L2_MEMORY_MMAP;
    
	if (-1 == xioctl(cam.fdv, VIDIOC_REQBUFS, &req)) {
		if (EINVAL == errno) {
			fprintf(stderr, "%s does not support "
                    "memory mapping\n", device);
			exit(EXIT_FAILURE);
		} else {
			errno_exit("VIDIOC_REQBUFS");
		}
	}
    
    if (req.count < 2) {
		fprintf(stderr, "Insufficient buffer memory on %s\n",
                device);
		exit(EXIT_FAILURE);
	}
    
    cam.buffers = calloc(req.count, sizeof(*cam.buffers));
    
	if (!cam.buffers) {
		fprintf(stderr, "Out of memory\n");
		exit(EXIT_FAILURE);
	}
    
    for (cam.n_buffers = 0; cam.n_buffers < req.count; ++cam.n_buffers)
    {
        CLEAR(buf);
        
        buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory      = V4L2_MEMORY_MMAP;
        buf.index       = cam.n_buffers;
        
        if (-1 == xioctl(cam.fdv, VIDIOC_QUERYBUF, &buf))
            errno_exit("VIDIOC_QUERYBUF");
        
		cam.buffers[cam.n_buffers].length = buf.length;
		cam.buffers[cam.n_buffers].start =
        mmap(NULL /* start anywhere */,
             buf.length,
             PROT_READ | PROT_WRITE /* required */,
             MAP_SHARED /* recommended */,
             cam.fdv, buf.m.offset);
        
		if (MAP_FAILED == cam.buffers[cam.n_buffers].start)
			errno_exit("mmap");
    }
    
    
    /* Start capturing */    
    for (i = 0; i < cam.n_buffers; ++i) {
        
        CLEAR(buf);
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = i;
        
        if (-1 == xioctl(cam.fdv, VIDIOC_QBUF, &buf))
            errno_exit("VIDIOC_QBUF");
    }
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (-1 == xioctl(cam.fdv, VIDIOC_STREAMON, &type))
        errno_exit("VIDIOC_STREAMON");
    
    /* Init frame rate calcul */
    cam.fps = 0;    
    gettimeofday(&cam.tv, NULL);
    
    return cam;
}

void v4l2cam_close(v4l2cam_t camera)
{
    unsigned int i;
    enum v4l2_buf_type type;
    
    /* Stop capturing */
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (-1 == xioctl(camera.fdv, VIDIOC_STREAMOFF, &type))
        errno_exit("VIDIOC_STREAMOFF");
    
    /* Uninit device */
    for (i = 0; i < camera.n_buffers; ++i)
        if (-1 == munmap(camera.buffers[i].start, camera.buffers[i].length))
            errno_exit("munmap");
    
    
	free(camera.buffers);
    
    /* Close device */
    if (-1 == close(camera.fdv))
		errno_exit("close");
    
    free_image(camera.framebuf);
    
	camera.fdv = -1;
}

void v4l2cam_grab_image(v4l2cam_t *camera)
{
    struct timeval t;
    
    for (;;) {
        fd_set fds;
        struct timeval tv;
        int r;
        
        FD_ZERO(&fds);
        FD_SET(camera->fdv, &fds);
        
        /* Timeout. */
        tv.tv_sec = 2;
        tv.tv_usec = 0;
        
        r = select(camera->fdv + 1, &fds, NULL, NULL, &tv);
        
        if (-1 == r) {
            if (EINTR == errno)
                continue;
            errno_exit("select");
        }
        
        if (0 == r) {
            fprintf(stderr, "select timeout\n");
            exit(EXIT_FAILURE);
        }
        
        if (read_frame(*camera))
            break;
        /* EAGAIN - continue select loop. */
    }
    
    gettimeofday(&t, NULL);
    camera->fps = 1000000.0 / ( (t.tv_sec - camera->tv.tv_sec)*1000000.0 + (t.tv_usec - camera->tv.tv_usec));
    camera->tv = t;
}
